#include "gimbal_behavior.h"

static int div_delta(int16_t red, int16_t target);

static gimbal_behavior_e gimbal_behaviour;
static int16_t counter = 0;
static int16_t counter_flag = 0;
static char Point_count=0;
static void max_speed_limit(fp32 * x_d, fp32* y_d);
static void one_dir_limit(fp32 * x_d, fp32* y_d);
void gimbal_behaviour_set_mode(Gimbal_Control_t *point_locat)
{
	static int  init_flag=0;
	
	static const fp32 pitch_pixel_pid[3] = {PITCH_PID_KP,PITCH_PID_KI,PITCH_PID_KD};
	static const fp32 yaw_pixel_pid[3] = {YAW_PID_KP,YAW_PID_KI,YAW_PID_KD};
	
	if(!init_flag) {
		PID_Init(&point_locat->gimbal_pitch_motor.gimbal_motor_pixel_pid,PID_POSITION,pitch_pixel_pid, PITCH_PID_MAX_OUT, PITCH_PID_MAX_IOUT);
		PID_Init(&point_locat->gimbal_yaw_motor.gimbal_motor_pixel_pid,PID_POSITION,yaw_pixel_pid, YAW_PID_MAX_OUT, YAW_PID_MAX_IOUT);
		init_flag = 1;
	}
	
	if((*(point_locat->now_mode)==GIMBAL_RESET_MODE)&&gimbal_behaviour!=GIMBAL_RESET_MODE)
		counter_flag = 1;
	else if((*(point_locat->now_mode)==GIMBAL_RECT_MODE)&&gimbal_behaviour!=GIMBAL_RECT_MODE&&gimbal_behaviour!=GIMBAL_STOP_MODE) {
		Point_count = 0;
		counter = 0;
	}
	gimbal_behaviour = *(point_locat->now_mode);

	
	
}

void gimbal_behaviour_control_set(fp32 *add_yaw_angle, fp32 * add_pitch_angle, Gimbal_Control_t *gimbal_set_control)
{
	*add_yaw_angle = 0;
	int i;
	*add_pitch_angle = 0;
	fp32 x_delta;
	fp32 y_delta;
	static fp32 x_delta_last=0;
	static fp32 y_delta_last=0;
	
	static point_locat point_rect[5];
	
	
	
	switch (gimbal_behaviour)
	{
		case GIMBAL_STOP_MODE:
			*add_yaw_angle = 0;
			*add_pitch_angle = 0;
		break;

#if GIMBAL_Task==GIMBAL_RED
		case GIMBAL_RESET_MODE:
			
			Point_count=0;
			if(gimbal_set_control->PI_Data->red_point.x!=0&&gimbal_set_control->PI_Data->red_point.y!=0) {
				x_delta = gimbal_set_control->PI_Data->red_point.x - gimbal_set_control->PI_Data->center_point.x;
				y_delta = gimbal_set_control->PI_Data->red_point.y - gimbal_set_control->PI_Data->center_point.y;
				x_delta_last = x_delta;
				y_delta_last = y_delta;
			}
			else {
				x_delta = 9;
				y_delta = 9;
			}
				
		
			if(x_delta<=ERROR_DEADZONE && x_delta>=-ERROR_DEADZONE && y_delta<=ERROR_DEADZONE && y_delta>=-ERROR_DEADZONE) {
				buzzer_on();
				if(counter_flag == 1) {
					for(i=0; i<4; i++) 
					{
						point_rect[i] = gimbal_set_control->PI_Data->point[i];
//						printf("%d %d\n",point_rect[0].y,point_rect[4].y);
					}
					point_rect[4] = point_rect[0];
					counter_flag = 0;
					
				}
				
				*add_yaw_angle = -PIXEL_TO_DELTA_DATA*x_delta;
				*add_pitch_angle = -PIXEL_TO_DELTA_DATA*y_delta;
//				printf("%d %d %d %.4f, %.4f\n",*(gimbal_set_control->now_mode),counter,Point_count,*add_yaw_angle,*add_pitch_angle);
				return;
			}
			else
				buzzer_off();
		

//			printf("%.2f  %.2f ",x_delta,y_delta);
			one_dir_limit(&x_delta,&y_delta);
//			printf("%.2f  %.2f ",x_delta,y_delta);
			*add_yaw_angle = -PIXEL_TO_DELTA_DATA*x_delta;
			*add_pitch_angle = -PIXEL_TO_DELTA_DATA*y_delta;
			
		break; 
		
		case GIMBAL_RECT_MODE:
			
			if(Point_count>=5&&counter<=0) {
				*(gimbal_set_control->now_mode) = 0;
//				printf("%d %d\n",point_rect[0].y,point_rect[4].y);
				buzzer_off();
				return;
			}
				
		
			
			
			if(counter>0) {
				counter--;
				buzzer_on();
//				x_delta = gimbal_set_control->PI_Data->red_point.x - point_rect[Point_count-1].x;
//				y_delta = gimbal_set_control->PI_Data->red_point.y - point_rect[Point_count-1].y;
				if(gimbal_set_control->PI_Data->red_point.x!=0&&gimbal_set_control->PI_Data->red_point.y!=0) {
					x_delta = gimbal_set_control->PI_Data->red_point.x - point_rect[Point_count-1].x;
					y_delta = gimbal_set_control->PI_Data->red_point.y - point_rect[Point_count-1].y;
					x_delta_last = x_delta;
					y_delta_last = y_delta;
				}
				else {
					x_delta = 9;
					y_delta = 9;
				}
				*add_yaw_angle = -PIXEL_TO_DELTA_DATA*x_delta;
				*add_pitch_angle = -PIXEL_TO_DELTA_DATA*y_delta;
				return;
			}
			else {
				buzzer_off();
//				x_delta = gimbal_set_control->PI_Data->red_point.x - point_rect[Point_count].x;
//				y_delta = gimbal_set_control->PI_Data->red_point.y - point_rect[Point_count].y;
				if(gimbal_set_control->PI_Data->red_point.x!=0&&gimbal_set_control->PI_Data->red_point.y!=0) {
					x_delta = gimbal_set_control->PI_Data->red_point.x - point_rect[Point_count].x;
					y_delta = gimbal_set_control->PI_Data->red_point.y - point_rect[Point_count].y;
					x_delta_last = x_delta;
					y_delta_last = y_delta;
				}
				else {
					x_delta = 9;
					y_delta = 9;
				}
			}
				
			
			if(x_delta<=ERROR_DEADZONE && x_delta>=-ERROR_DEADZONE && y_delta<=ERROR_DEADZONE && y_delta>=-ERROR_DEADZONE) {
				
				if(counter<=0) {
					Point_count ++;
					counter = DELAY_CNT;
					printf("point_up");
				}
				else {
					printf("ZERO_ZONE!\n");
				}
			}

			
			
			

//			printf("%.2f  %.2f ",x_delta,y_delta);
			one_dir_limit(&x_delta,&y_delta);
//			printf(" %.2f  %.2f\n",x_delta,y_delta);
			*add_yaw_angle = -PIXEL_TO_DELTA_DATA*x_delta;
			*add_pitch_angle = -PIXEL_TO_DELTA_DATA*y_delta;
			
		break;
#endif
#if     GIMBAL_Task==GIMBAL_BLUE
		case GIMBAL_TRCK_MODE:
			x_delta = (gimbal_set_control->PI_Data->red_green_delta.x_delta);
			y_delta = (gimbal_set_control->PI_Data->red_green_delta.y_delta);
		
//			x_delta = *add_yaw_angle;
//			y_delta = *add_pitch_angle;
		
			if(x_delta<=ERROR_DEADZONE && x_delta>=-ERROR_DEADZONE && y_delta<=ERROR_DEADZONE && y_delta>=-ERROR_DEADZONE && y_delta!= 0 && x_delta !=0) {
				buzzer_on();
			}
			else
				buzzer_off();
			
			
//			if(x_delta > PIXEL_DEADZONE || x_delta < -PIXEL_DEADZONE)
//				x_delta = div_delta(gimbal_set_control->PI_Data->red_green_delta.x_delta, 0);
//			
//			if(y_delta > PIXEL_DEADZONE || y_delta < -PIXEL_DEADZONE)
//				y_delta = div_delta(gimbal_set_control->PI_Data->red_green_delta.y_delta, 0);
			printf("%d  %d  %f  %f\n ",gimbal_set_control->PI_Data->red_green_delta.x_delta,gimbal_set_control->PI_Data->red_green_delta.y_delta,x_delta,y_delta);
			*add_yaw_angle = PIXEL_TO_DELTA_DATA*x_delta;
			*add_pitch_angle = PIXEL_TO_DELTA_DATA*y_delta;
			
		break;
#endif
			
			
		default:
//			*add_yaw_angle = 0;
//			*add_pitch_angle = 0;
			*(gimbal_set_control->now_mode) = 0;
		break;
		
	}
	
//	printf("%d %d %d %.4f, %.4f\n",*(gimbal_set_control->now_mode),counter,Point_count,*add_yaw_angle,*add_pitch_angle);
}

static int div_delta(int16_t red, int16_t target)
{
	int16_t real_target;
	real_target = PIXEL_LOW_PASS*red + (1 - PIXEL_LOW_PASS)*target;
	
	return (red - real_target);
	
}

static void max_speed_limit(fp32 * x_d, fp32* y_d)
{
	
	fp32 x_speed = (*x_d);
	fp32 y_speed = (*y_d);
	fp32 last_kp;
	
	fp32 speed_all_2 = x_speed*x_speed + y_speed*y_speed;
	
	fp32 speed_kp;
	
	fp32 x_speed_kp, y_speed_kp;
	
	if(speed_all_2>=EEROR_ALL_MAX_2||speed_all_2<=-EEROR_ALL_MAX_2)
	
		speed_kp = EEROR_ALL_MAX_2/speed_all_2;   
	else
		speed_kp = 1;
	
	x_speed = speed_kp*x_speed;
	y_speed = speed_kp*y_speed;
	
	if(x_speed>=0)
		x_speed_kp = x_speed/ERROR_MIN;
	else
		x_speed_kp = -x_speed/ERROR_MIN;
	
	if(y_speed>=0)
		y_speed_kp = y_speed/ERROR_MIN;
	else
		y_speed_kp = -y_speed/ERROR_MIN;
//	y_speed_kp = y_speed/ERROR_MIN;
	if(x_speed_kp==0)
		last_kp = y_speed_kp;
	else if(y_speed_kp==0)
		last_kp = x_speed_kp;
	else
		last_kp = x_speed_kp>y_speed_kp?y_speed_kp:x_speed_kp;
	
	if(last_kp<1&&last_kp!=0) {
		x_speed = x_speed/last_kp;
		y_speed = y_speed/last_kp;
	}

	* x_d = x_speed;
	* y_d = y_speed;
	
	
		
}


static void one_dir_limit(fp32 * x_d, fp32* y_d)
{
	fp32 speed_x = (*x_d);
	fp32 speed_y = (*y_d);
	
//	printf("%.2f, %.2f ",(*x_d),(*y_d));
	
	fp32 speed_x_kp;
	fp32 speed_y_kp;
	
	fp32 last_kp;
	
	if(speed_x>0)
		speed_x_kp = speed_x/ERROR_MAX;
	else if(speed_x<0)
		speed_x_kp = -speed_x/ERROR_MAX;
	
	if(speed_y>0)
		speed_y_kp = speed_y/ERROR_MAX;
	else if(speed_x<0)
		speed_y_kp = -speed_y/ERROR_MAX;
	 
//	printf("%.2f, %.2f ",speed_x_kp,speed_y_kp);
	
	if(speed_x_kp>1||speed_y_kp>1) {
		last_kp = speed_x_kp>speed_y_kp?speed_x_kp:speed_y_kp;
		speed_x = speed_x/last_kp;
		speed_y = speed_y/last_kp;
		* x_d = speed_x;
		* y_d = speed_y;
		speed_x_kp = 0;
		speed_y_kp = 0;
	}
	
//	printf("%.2f, %.2f,  %.2f,  %.2f  %.2f\n",(speed_x_kp),(speed_y_kp),last_kp,(*x_d),(*y_d));
//	else if(speed_x_kp>1&&speed_y_kp<1) {
//		speed_x = speed_x/speed_x_kp;
//		* x_d = speed_x;
//	}
//	else if(speed_y_kp>1&&speed_x_kp<1) {
//		speed_y = speed_y/speed_y_kp;
//		* x_d = speed_y;
//	}
//	else
//	{
//		
//	}
	
	
	
	
	
}



